/*
 * ......................................&&.........................
 * ....................................&&&..........................
 * .................................&&&&............................
 * ...............................&&&&..............................
 * .............................&&&&&&..............................
 * ...........................&&&&&&....&&&..&&&&&&&&&&&&&&&........
 * ..................&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&..............
 * ................&...&&&&&&&&&&&&&&&&&&&&&&&&&&&&.................
 * .......................&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&.........
 * ...................&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&...............
 * ..................&&&   &&&&&&&&&&&&&&&&&&&&&&&&&&&&&............
 * ...............&&&&&@  &&&&&&&&&&..&&&&&&&&&&&&&&&&&&&...........
 * ..............&&&&&&&&&&&&&&&.&&....&&&&&&&&&&&&&..&&&&&.........
 * ..........&&&&&&&&&&&&&&&&&&...&.....&&&&&&&&&&&&&...&&&&........
 * ........&&&&&&&&&&&&&&&&&&&.........&&&&&&&&&&&&&&&....&&&.......
 * .......&&&&&&&&.....................&&&&&&&&&&&&&&&&.....&&......
 * ........&&&&&.....................&&&&&&&&&&&&&&&&&&.............
 * ..........&...................&&&&&&&&&&&&&&&&&&&&&&&............
 * ................&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&............
 * ..................&&&&&&&&&&&&&&&&&&&&&&&&&&&&..&&&&&............
 * ..............&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&....&&&&&............
 * ...........&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&......&&&&............
 * .........&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&.........&&&&............
 * .......&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&...........&&&&............
 * ......&&&&&&&&&&&&&&&&&&&...&&&&&&...............&&&.............
 * .....&&&&&&&&&&&&&&&&............................&&..............
 * ....&&&&&&&&&&&&&&&.................&&...........................
 * ...&&&&&&&&&&&&&&&.....................&&&&......................
 * ...&&&&&&&&&&.&&&........................&&&&&...................
 * ..&&&&&&&&&&&..&&..........................&&&&&&&...............
 * ..&&&&&&&&&&&&...&............&&&.....&&&&...&&&&&&&.............
 * ..&&&&&&&&&&&&&.................&&&.....&&&&&&&&&&&&&&...........
 * ..&&&&&&&&&&&&&&&&..............&&&&&&&&&&&&&&&&&&&&&&&&.........
 * ..&&.&&&&&&&&&&&&&&&&&.........&&&&&&&&&&&&&&&&&&&&&&&&&&&.......
 * ...&&..&&&&&&&&&&&&.........&&&&&&&&&&&&&&&&...&&&&&&&&&&&&......
 * ....&..&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&...........&&&&&&&&.....
 * .......&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&..............&&&&&&&....
 * .......&&&&&.&&&&&&&&&&&&&&&&&&..&&&&&&&&...&..........&&&&&&....
 * ........&&&.....&&&&&&&&&&&&&.....&&&&&&&&&&...........&..&&&&...
 * .......&&&........&&&.&&&&&&&&&.....&&&&&.................&&&&...
 * .......&&&...............&&&&&&&.......&&&&&&&&............&&&...
 * ........&&...................&&&&&&.........................&&&..
 * .........&.....................&&&&........................&&....
 * ...............................&&&.......................&&......
 * ................................&&......................&&.......
 * .................................&&..............................
 * ..................................&..............................
 */

#include "Data_MATH.h"
#include "d_math.h"

/************************************************************万能分隔符**************************************************************
 * 	@author:			//小瑞
 *	@performance:	    //int16_t绝对值
 *	@parameter:		    //
 *	@time:				//23-04-20 03:38
 *	@ReadMe:			//
 ************************************************************万能分隔符**************************************************************/
int16_t MATH_ABS_int16_t(int16_t DATA)
{

    return DATA >> 15 == 0 ? DATA : (~DATA + 1);
}
int32_t MATH_ABS_int32_t(int32_t DATA)
{
    if(DATA < 0)
        return -DATA;
    if(DATA > 0)
        return DATA;
    return 0;
}

/************************************************************万能分隔符**************************************************************
 * 	@author:			//小瑞
 *	@performance:	    //float绝对值
 *	@parameter:		    //
 *	@time:				//23-04-20 03:39
 *	@ReadMe:			//
 ************************************************************万能分隔符**************************************************************/
float MATH_ABS_float(float DATA)
{
    uint32_t V_TEMP = *(uint32_t*)&DATA;
    V_TEMP &= 0x7FFFFFFF;  // 将符号位清零
    return *(float*)&V_TEMP;
}

/************************************************************万能分隔符**************************************************************
 * 	@author:			//小瑞
 *	@performance:	    //float 限幅
 *	@parameter:		    //
 *	@time:				//23-04-20 03:39
 *	@ReadMe:			//
 ************************************************************万能分隔符**************************************************************/
float MATH_Limit_float(float MAX, float MIN, float DATA)
{
    return (DATA > MAX) ? MAX : ((DATA < MIN) ? MIN : DATA);
}
/************************************************************万能分隔符**************************************************************
 * 	@author:			//小瑞
 *	@performance:	    //float 限幅
 *	@parameter:		    //
 *	@time:				//2023-05-23 17:41
 *	@ReadMe:			//
 ************************************************************万能分隔符**************************************************************/
int16_t MATH_Limit_int16(int16_t MAX, int16_t MIN, int16_t DATA)
{
    return (DATA > MAX) ? MAX : ((DATA < MIN) ? MIN : DATA);
}
/************************************************************万能分隔符**************************************************************
 * 	@author:			//小瑞
 *	@performance:	    //SETBIT
 *	@parameter:
 * //@设置的字节的指针@要设置的位所在的位置（0到7），@要设置的值（1表示要将指定位设置为1，0表示要将其设置为0）
 *	@time:				//23-04-20 02:28
 *	@ReadMe:			//
 ************************************************************万能分隔符**************************************************************/
void MATH_SETBIT(unsigned char* byte, int position, int value)
{
    unsigned char mask = 1 << position;  // 生成一个只有指定位置为1的掩码
    if(value)
    {
        *byte |= mask;  // 将指定位置设置为1
    }
    else
    {
        *byte &= ~mask;  // 将指定位置设置为0
    }
}
/************************************************************万能分隔符**************************************************************
 * 	@author:			//小瑞
 *	@performance:	    //float平方根倒数
 *	@parameter:		    //
 *	@time:				//23-04-20 10:20
 *	@ReadMe:			//
 ************************************************************万能分隔符**************************************************************/
float MATH_INV_SQRT_float(float DATA)
{
    float DATA_half = 0.5f * DATA;
    uint32_t i = *(uint32_t*)&DATA;                  // 将浮点数视为无符号整数
    i = 0x5f3759df - (i >> 1);                       // 运用魔数进行处理
    DATA = *(float*)&i;                              // 再将无符号整数转回浮点数
    DATA = DATA * (1.5f - DATA_half * DATA * DATA);  // 进行牛顿迭代
    return DATA;
}

// float uint_to_float(int16_t x_int, float span, int16_t value)
//{
//	return x_int-value/2/value*span;
// }
float Hex_To_Float(const uint32_t* Byte)  // 十六进制到浮点数
{
    return *((float*)Byte);
}

uint32_t FloatTohex(float HEX)  // 浮点数到十六进制转换
{
    return *(uint32_t*)&HEX;
}

/**
************************************************************************
* @brief:      	float_to_uint: 浮点数转换为无符号整数函数
* @param[in]:   x_float:	待转换的浮点数
* @param[in]:   x_min:		范围最小值
* @param[in]:   x_max:		范围最大值
* @param[in]:   bits: 		目标无符号整数的位数
* @retval:     	无符号整数结果
* @details:    	将给定的浮点数 x 在指定范围 [x_min, x_max] 内进行线性映射，映射结果为一个指定位数的无符号整数
************************************************************************
**/
int float_to_uint(float x_float, float x_min, float x_max, int bits)
{
    /* Converts a float to an unsigned int, given range and number of bits */
    float span = x_max - x_min;
    float offset = x_min;
    return (int)((x_float - offset) * ((float)((1 << bits) - 1)) / span);
}
/**
************************************************************************
* @brief:      	uint_to_float: 无符号整数转换为浮点数函数
* @param[in]:   x_int: 待转换的无符号整数
* @param[in]:   x_min: 范围最小值
* @param[in]:   x_max: 范围最大值
* @param[in]:   bits:  无符号整数的位数
* @retval:     	浮点数结果
* @details:    	将给定的无符号整数 x_int 在指定范围 [x_min, x_max] 内进行线性映射，映射结果为一个浮点数
************************************************************************
**/
float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
    /* converts unsigned int to float, given range and number of bits */
    float span = x_max - x_min;
    float offset = x_min;
    return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}

/*限制角度在0-2PI之间*/
/*限制角度在0-2PI之间*/
float Limit_Angle(float ElAngle)
{
    if(ElAngle > M_2PI)
    {
        ElAngle -= M_2PI;
    }
    else if(ElAngle < 0)
    {
        ElAngle += M_2PI;
    }
    return ElAngle;
}
